#include "physicsActor.h"
#include "renderer.h"
#include "input.h"
#include "ragDoll.h"

PhysicsActor::PhysicsActor(){

name="PhysicsActor";
world=renderer->physicsWorld;
space=renderer->collisionSpace;

shape=Vector3f(1.0,1.0,1.0);
mass=1.0;
type=CAPSULESHAPE;
drawing=new ParticleSystem;

registerProperties();
}

PhysicsActor::PhysicsActor(Vector3f myShape,dReal myMass, int myType){

world=renderer->physicsWorld;
space=renderer->collisionSpace;

shape=myShape;
type=myType;
mass=myMass;

free(drawing);
}

PhysicsActor::~PhysicsActor(){


}


void PhysicsActor::setup(){

 	body = dBodyCreate(world);

	dMass m;

    if (type==CUBESHAPE)
      {
	  geom = dCreateBox(space,shape.x,shape.y,shape.z);
      dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z);
      }
	if (type==CAPSULESHAPE)
	  {
	  geom = dCreateCapsule(space,shape.x,shape.y);
      dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y);      //the '3' means align on z-axis  and density is shape.z
	  generateCapsuleList();
	  }
	dMassAdjust(&m,mass);
	dBodySetMass(body,&m);

	dGeomSetBody(geom,body);
	setLocation(location);
	setRotation(rotation);
}

void PhysicsActor::update(double deltaTime){}


void PhysicsActor::setLocation(Vector3f loc){

    //see if we need to free ourselves from our parent, so we can update our dependencies correctly!
    Actor::setLocation(loc);

    // Equivalent to dBodySetPosition(geom,x,y,z);
	dGeomSetPosition(geom,loc.x,loc.y,loc.z);
	if (parent!=NULL){
      if (parent->isA("RAGDOLL"))
        ((RagDoll*)parent)->updateString(name, location);
	}


}

void PhysicsActor::setRotationVec(Vector3f rot){

float xAngle=rot.dotProduct(Vector3f(1,0,0));
float yAngle=rot.dotProduct(Vector3f(0,1,0));
float zAngle=rot.dotProduct(Vector3f(0,0,1));

setRotation(Vector3f(xAngle,yAngle,zAngle));
}


void PhysicsActor::setRotation(Vector3f rot){

    Actor::setRotation(rot);
	// Equivalent to dGeomSetPosition(geom,x,y,z);
	dMatrix3 rotMatrix;
    dMatrix4 modelView;

	//use OpenGL to do the matrix transforms for us
	glMatrixMode(GL_MODELVIEW);

	glPushMatrix();
    glLoadIdentity();

    glRotatef(rot.x,1,0,0);
    glRotatef(rot.y,0,1,0);
    glRotatef(rot.z,0,0,1);

    glGetFloatv(GL_MODELVIEW_MATRIX , modelView);
	glPopMatrix();
/*
	rotMatrix[ 0] = 0;  rotMatrix[ 1] = -1;  rotMatrix[ 2] = 0; rotMatrix[ 3] = 0;
	rotMatrix[ 4] = 1;  rotMatrix[ 5] = 0;	rotMatrix[ 6] = 0; rotMatrix[ 7] = 0;
	rotMatrix[ 8] = 0;  rotMatrix[ 9] = 0;  rotMatrix[10] = 1; rotMatrix[11] = 0;
*/
	rotMatrix[ 0] = modelView[ 0];  rotMatrix[ 1] = modelView[ 4];  rotMatrix[ 2] = modelView[ 8];
	rotMatrix[ 4] = modelView[ 1];  rotMatrix[ 5] = modelView[ 5];  rotMatrix[ 6] = modelView[ 9];
	rotMatrix[ 8] = modelView[ 2];  rotMatrix[ 9] = modelView[ 6];  rotMatrix[ 10] = modelView[10];

	dBodySetRotation(body,rotMatrix);
}

void PhysicsActor::draw(){


	glPushMatrix();

    if (base)
        glLoadMatrixf(base->baseMatrix);


	const dReal *p = dBodyGetPosition(body);
	const dReal *r = dBodyGetRotation(body);
	float m[16];
	m[ 0] = r[ 0];m[ 1] = r[ 4];m[ 2] = r[ 8];m[ 3] = 0;
	m[ 4] = r[ 1];m[ 5] = r[ 5];m[ 6] = r[ 9];m[ 7] = 0;
	m[ 8] = r[ 2];m[ 9] = r[ 6];m[10] = r[10];m[11] = 0;
	m[12] = p[ 0];m[13] = p[ 1];m[14] = p[ 2];m[15] = 1;
	glMultMatrixf(m);


	if (drawing)
	  drawParticles();


	//if (!input->controller->bRunning)  //todo:find a solution for this
	  drawCollisionBox();


	glPopMatrix();
}

void PhysicsActor::drawCollision(){

draw();
}


#define CAPSULE_SLICES 16
#define CAPSULE_STACKS 12

void PhysicsActor::drawCollisionBox() const {

	if (type==CUBESHAPE)
	  {
	  glScalef(shape.x,shape.y,shape.z);
      glColor4fv(color);
      glutSolidCube(0.5);
	  }

    if (type==CAPSULESHAPE)
	  {
	  glCallList(capsuleList);
	  }
    //TODO: for painting purposes
    if (type==FLATSHAPE)
      {
      glScalef(shape.x,shape.y,shape.z);
      glColor4fv(color);
      glBegin(GL_QUADS);
		glTexCoord2f(0.0,0.0);
		glVertex3f(0,0,0.0);
        glTexCoord2f(1.0,0.0);
        glVertex3f(scale.x,0,0.0);
        glTexCoord2f(1.0,1.0);
		glVertex3f(scale.x,scale.y,0.0);
        glTexCoord2f(0.0,1.0);
		glVertex3f(0,scale.y,0.0);
      glEnd();
      }
}

void PhysicsActor::drawParticles(){


    if (drawing->bTextured)
      {
      if (drawing->textureID!="") glBindTexture(GL_TEXTURE_2D, renderer->textureList[drawing->textureID]->texture);
      if (drawing->bShadowTextured) glBindTexture(GL_TEXTURE_2D,renderer->textureList["shadowTexture"]->texture);
      }

    renderer->setupShading(drawing->sceneShaderID);
    drawing->updateShaders();

    if (!drawing->bZTest) glDepthMask(GL_FALSE);


    glColor4f(drawing->color.r,drawing->color.g,drawing->color.b,drawing->color.a);
    drawing->drawParticles();

    if (!drawing->bZTest) glDepthMask(GL_TRUE);
}


void PhysicsActor::generateCapsuleList(){

capsuleList=glGenLists(1);

glNewList(capsuleList,GL_COMPILE);

	  glColor4fv(color);
	  float cylHalfHeight = shape.y / 2.0;
	  glBegin(GL_QUAD_STRIP);
		for (int i=0; i<(CAPSULE_SLICES + 1);i++)
		  {
			float angle = float(i) / float(CAPSULE_SLICES) * 2.0 * M_PI;
			float ca = cos(angle);
			float sa = sin(angle);
			glNormal3f(ca, sa, 0);
			glVertex3f(shape.x * ca, shape.x * sa, cylHalfHeight);
			glVertex3f(shape.x * ca, shape.x * sa, -cylHalfHeight);
		  }

		glEnd();
		glTranslatef(0, 0, cylHalfHeight);
		glutSolidSphere(shape.x, CAPSULE_SLICES, CAPSULE_STACKS);
		glTranslatef(0, 0, -2.0 * cylHalfHeight);
		glutSolidSphere(shape.x, CAPSULE_SLICES, CAPSULE_STACKS);
// endList
glEndList();

}

void PhysicsActor::setRotationODE(Vector3f vec){

/*
# define body rotation automatically from body axis
		za = norm3(sub3(p2, p1))
		if (abs(dot3(za, (1.0, 0.0, 0.0))) < 0.7): xa = (1.0, 0.0, 0.0)
		else: xa = (0.0, 1.0, 0.0)
		ya = cross(za, xa)
		xa = norm3(cross(ya, za))
		ya = cross(za, xa)
		rot = (xa[0], ya[0], za[0], xa[1], ya[1], za[1], xa[2], ya[2], za[2])

*/

        Vector3f xAxis;
		Vector3f yAxis;
		Vector3f zAxis;

		zAxis = vec;
		zAxis.normalize();

		//check forward/backward facing ?
		if (abs(zAxis.dotProduct(Vector3f(1.0, 0.0, 0.0))) < 0.7)
		  xAxis = Vector3f(1.0, 0.0, 0.0);
		else
		  xAxis = Vector3f(0.0, 1.0, 0.0);

		yAxis = zAxis.crossProduct(xAxis);

		xAxis = yAxis.crossProduct(zAxis);
		xAxis.normalize();

		yAxis = zAxis.crossProduct(xAxis);

		dMatrix3 rotMatrix;
		rotMatrix[0] = xAxis[0]; rotMatrix[1] = yAxis[0]; rotMatrix[2] = zAxis[0];
        rotMatrix[4] = xAxis[1]; rotMatrix[5] = yAxis[1]; rotMatrix[6] = zAxis[1];
        rotMatrix[8] = xAxis[2]; rotMatrix[9] = yAxis[2]; rotMatrix[10] = zAxis[2];

        dBodySetRotation(body,rotMatrix);
}


TiXmlElement* PhysicsActor::save(TiXmlElement *root){

TiXmlElement* element=Actor::save(root);
TiXmlElement* drawingElement=drawing->save(element);
element->LinkEndChild(drawingElement);
return element;
}

void PhysicsActor::load(TiXmlElement *myInfo){

    Actor::load(myInfo);
    cout << "loading PhysicsActor!" << endl;
    TiXmlElement * element;
    element=myInfo->FirstChildElement( "Actor" );
    if (element)
      {
      drawing->load(element);
      drawing->setup();
      }

//no special loading required;
}
